

//================================================================================
LOCO_FORCE_INLINE Quaternion::Quaternion(float x_val, float y_val, float z_val, float w_val) : x(x_val), y(y_val), z(z_val), w(w_val)
{

}

//================================================================================
LOCO_FORCE_INLINE Quaternion::Quaternion(Vector3 const& unit_axis, float angle)
{
	float sin_half_angle = sin(0.5f * angle);
	x = unit_axis.x * sin_half_angle;
	y = unit_axis.y * sin_half_angle;
	z = unit_axis.z * sin_half_angle;
	w = cos(0.5f * angle);
}

//================================================================================
LOCO_FORCE_INLINE Quaternion::Quaternion(Matrix4 const& m)
{
	float	s;

	// Check diagonal (trace)
	const float tr = m.m[0][0] + m.m[1][1] + m.m[2][2];

	if (tr > 0.0f)
	{
		float inv_s = 1.0f / sqrt(tr + 1.0f);
		this->w = 0.5f * (1.f / inv_s);
		s = 0.5f * inv_s;

		this->x = (m.m[2][1] - m.m[1][2]) * s;
		this->y = (m.m[0][2] - m.m[2][0]) * s;
		this->z = (m.m[1][0] - m.m[0][1]) * s;
	}
	else
	{
		// diagonal is negative
		int32 i = 0;

		if (m.m[1][1] > m.m[0][0])
			i = 1;

		if (m.m[2][2] > m.m[i][i])
			i = 2;

		static const int32 nxt[3] = { 1, 2, 0 };
		const int32 j = nxt[i];
		const int32 k = nxt[j];

		s = m.m[i][i] - m.m[j][j] - m.m[k][k] + 1.0f;

		float inv_s = 1.0f / sqrt(s);

		float qt[4];
		qt[i] = 0.5f * (1.f / inv_s);

		s = 0.5f * inv_s;

		qt[3] = (m.m[k][j] - m.m[j][k]) * s;
		qt[j] = (m.m[j][i] + m.m[i][j]) * s;
		qt[k] = (m.m[k][i] + m.m[i][k]) * s;

		this->x = qt[0];
		this->y = qt[1];
		this->z = qt[2];
		this->w = qt[3];
	}
}

//================================================================================
LOCO_FORCE_INLINE Quaternion Quaternion::operator*(Quaternion const& in) const
{
	Quaternion result;
	result.x = w * in.x + in.w * x + (y * in.z - z * in.y);
	result.y = w * in.y + in.w * y + (z * in.x - x * in.z);
	result.z = w * in.z + in.w * z + (x * in.y - y * in.x);
	result.w = w * in.w - (x * in.x + y * in.y + z * in.z);
	return result;
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::axis() const
{
	float sin_half_angle_inv = 1.0f / sin(0.5f * angle());
	return Vector3(x * sin_half_angle_inv, y * sin_half_angle_inv, z * sin_half_angle_inv);
}

//================================================================================
LOCO_FORCE_INLINE float Quaternion::angle() const
{
	return 2.0f * acos(w);
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::euler() const
{
	Vector3 euler;
	float singularity_test = z*x - w*y;
	float pitch = asin(2 * singularity_test);

	euler.x = atan2(2.0f*(w*z + x*y), (1 - 2.0f*(y*y + z*z)))*RAD_TO_DEG;

	#define SINGULARITY_THRESHOLD	0.4999995f

	if (singularity_test < -SINGULARITY_THRESHOLD)
	{
		euler.z = -euler.x - 2.0f* atan2(x, w) * RAD_TO_DEG;
		euler.y = 270.0f;
	}
	else if (singularity_test > SINGULARITY_THRESHOLD)
	{
		euler.z = euler.x - 2.0f* atan2(x, w) * RAD_TO_DEG;
		euler.y = 90.0f;
	}
	else
	{
		euler.z = atan2(-2.0f*(w*x + y*z), (1 - 2.0f*(x*x + y*y)))*RAD_TO_DEG;

		euler.y = fmod(pitch*RAD_TO_DEG, 360.0f);
		if (euler.y < 0.0f)
			euler.y += 360.0f;
	}

	// normalize euler angle (every angle between -360 and 360)
	euler.x = fmod(euler.x, 360.0f);
	if (euler.x < 0.0f)
		euler.x += 360.0f;

	euler.y = fmod(euler.y, 360.0f);
	if (euler.y < 0.0f)
		euler.y += 360.0f;

	euler.z = fmod(euler.z, 360.0f);
	if (euler.z = 0.0f)
		euler.z += 360.0f;

	return euler;
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::x_axis() const
{
	return (*this).rotate(Vector3(1.0f, 0.0f, 0.0f));
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::y_axis() const
{
	return (*this).rotate(Vector3(0.0f, 1.0f, 0.0f));
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::z_axis() const
{
	return (*this).rotate(Vector3(0.0f, 0.0f, 1.0f));
}

//================================================================================
LOCO_FORCE_INLINE Vector3 Quaternion::rotate(Vector3 const& in) const
{
	Vector3 qv(x, y, z);
	Vector3 result = (Vector3::cross(qv, in)) * (2.0f * w);
	result += in * ((w * w) - Vector3::dot(qv, qv));
	result += qv * (Vector3::dot(qv, in) * 2.0f);
	return result;
}

//================================================================================
LOCO_FORCE_INLINE Quaternion Quaternion::inverse() const
{
	return Quaternion(-x, -y, -z, w);
}

//================================================================================
LOCO_FORCE_INLINE bool Quaternion::near_equal(Quaternion const& a, Quaternion const& b, float delta)
{
	return	loco::near_equal(a.x, b.x, delta) &&
		loco::near_equal(a.y, b.y, delta) &&
		loco::near_equal(a.z, b.z, delta) &&
		loco::near_equal(a.w, b.w, delta);
}

//================================================================================
LOCO_FORCE_INLINE Quaternion Quaternion::lerp(Quaternion const& a, Quaternion const& b, float value)
{
	Vector4 va(a.x, a.y, a.z, a.w);
	Vector4 vb(b.x, b.y, b.z, b.w);
	
	Vector4 vq = va * (1.0f - value) + vb * value;
	vq /= vq.norm();

	return Quaternion(vq.x, vq.y, vq.z, vq.w);
}

//================================================================================
LOCO_FORCE_INLINE Quaternion Quaternion::slerp(Quaternion const& a, Quaternion const& b, float value)
{
	Vector4 va(a.x, a.y, a.z, a.w);
	Vector4 vb(b.x, b.y, b.z, b.w);

	float theta = acos(Vector4::dot(va, vb));
	float fa = sin((1.0f - value)*theta)/sin(theta);
	float fb = sin(value * theta)/sin(theta);

	Vector4 vq = va*fa + vb*fb;

	return Quaternion(vq.x, vq.y, vq.z, vq.w);
}